#include "drv_dm.h"

// uint8_t DM_Init(motor_dm_t* dm_ptr)
// {
//   // 检查指针有效性
//   if(dm_ptr == NULL) {
//     return 0;     // 失败
//   }

//   // 将配置写入电机
//   uint8_t flag = 1;
//   // 1.设置控制模式(寄存器10)
//   if(dm_ptr->write_lock != 0) {
//     HAL_Delay(200);
//   }
  
//   dm_ptr->write_lock = 1;
//   uint8_t data[4] = {dm_ptr->mode, 0, 0, 0};
//   flag &= DM_WriteReg(dm_ptr->id, DM_RID_CTRL_MODE, data);
//   HAL_Delay(1);
//   flag &= DM_SaveReg(dm_ptr->id, DM_RID_CTRL_MODE);
//   HAL_Delay(10);
//   dm_ptr->write_lock = 0;

//   // 2. 设置位置范围 (寄存器21 - PMAX)
//   if(dm_ptr->write_lock != 0) {
//     HAL_Delay(200);
//   }
//   dm_ptr->write_lock = 1;
//   uint8_t* pbuf = (uint8_t*)&dm_ptr->pos_max;
//   data[0] = pbuf[0]; data[1] = pbuf[1];
//   data[2] = pbuf[2]; data[3] = pbuf[3]; 
//   flag &= DM_WriteReg(dm_ptr->id, DM_RID_PMAX, data);
//   HAL_Delay(1);
//   flag &= DM_SaveReg(dm_ptr->id, DM_RID_PMAX);
//   HAL_Delay(10);
//   dm_ptr->write_lock = 0;

//   // 3. 设置速度范围 (寄存器22 - VMAX)
//   if(dm_ptr->write_lock != 0) {
//     HAL_Delay(200);
//   }
//   dm_ptr->write_lock = 1;
//   uint8_t* vbuf = (uint8_t*)&dm_ptr->vel_max;
//   data[0] = vbuf[0]; data[1] = vbuf[1]; 
//   data[2] = vbuf[2]; data[3] = vbuf[3]; 
//   flag &= DM_WriteReg(dm_ptr->id, DM_RID_VMAX, data);
//   HAL_Delay(1);
//   flag &= DM_SaveReg(dm_ptr->id, DM_RID_VMAX);
//   HAL_Delay(10);
//   dm_ptr->write_lock = 0;

//   // 4. 设置波特率 (寄存器35 - can_br, can_br_1m: 4)
//   if(dm_ptr->write_lock != 0) {
//     HAL_Delay(200);
//   }
//   dm_ptr->write_lock = 1;
//   data[0] = DM_CAN_BR_1M; data[1] = 0; data[2] = 0; data[3] = 0; 
//   flag &= DM_WriteReg(dm_ptr->id, DM_RID_CAN_BR, data);
//   HAL_Delay(1);
//   flag &= DM_SaveReg(dm_ptr->id, DM_RID_CAN_BR);
//   HAL_Delay(10);
//   dm_ptr->write_lock = 0;
  
//   return flag;
// }

void DM_Init(motor_dm_t* dm_ptr, uint16_t id, uint16_t mst_id, dm_mode_enum mode)
{
  dm_ptr->id = id;
  dm_ptr->mst_id = mst_id;
  dm_ptr->mode = mode;
  dm_ptr->pos_max = DM_POS_FLOAT_MAX;
  dm_ptr->vel_max = DM_VEL_FLOAT_MAX;
  switch (mode){
    case mit_mode: 
      dm_ptr->mode_offset = mit_mode_offset;
      break;
    case pos_mode:
      dm_ptr->mode_offset = pos_mode_offset;
      break;
    case spd_mode:
      dm_ptr->mode_offset = spd_mode_offset;
      break;
    default:
      break;
  }
}

/**
* @brief:      	DM_Enable: 	DM4310电机初始化并写入
* @param[in]:   dm_ptr:     DM4310电机结构体指针，取用电机CAN ID
* @retval:     	uint8_t:    状态 (0:失败, 1:成功)
* @details:    	使能DM4310电机
**/
uint8_t DM_Enable(motor_dm_t* dm_ptr)
{	
	uint8_t data[8];
	uint16_t id = dm_ptr->id + dm_ptr->mode_offset;
	data[0] = 0xFF;
	data[1] = 0xFF;
	data[2] = 0xFF;
	data[3] = 0xFF;
	data[4] = 0xFF;
	data[5] = 0xFF;
	data[6] = 0xFF;
	data[7] = 0xFC;
	canx_send_data(&hcan1, id, data, 8);
	return 1;
}

/**
* @brief:      	DM_Disable: 禁用电机模式
* @param[in]:   dm_ptr:  	  DM4310电机结构体指针，取用电机CAN ID
* @retval:     	uint8_t:    初始化状态 (0:失败, 1:成功)
* @details:    	通过CAN总线向特定电机发送禁用特定模式的命令
**/
uint8_t DM_Disable(motor_dm_t* dm_ptr)
{
	uint8_t data[8];
	uint16_t id = dm_ptr->id + dm_ptr->mode_offset;
	
	data[0] = 0xFF;
	data[1] = 0xFF;
	data[2] = 0xFF;
	data[3] = 0xFF;
	data[4] = 0xFF;
	data[5] = 0xFF;
	data[6] = 0xFF;
	data[7] = 0xFD;
	
	canx_send_data(&hcan1, id, data, 8);
	return 1;
}


uint8_t DM_ResetZero(motor_dm_t* dm_ptr)
{
  uint8_t data[8];
	uint16_t id = dm_ptr->id + dm_ptr->mode_offset;
	
	data[0] = 0xFF;
  data[1] = 0xFF;
  data[2] = 0xFF;
  data[3] = 0xFF;
  data[4] = 0xFF;
  data[5] = 0xFF;
  data[6] = 0xFF;
  data[7] = 0xFE;
	
	canx_send_data(&hcan1, id, data, 8);
	return 1;
}

uint8_t DM_DriveSpeedCtrl(CAN_HandleTypeDef* hcan, uint16_t motor_id, float vel)
{
  uint16_t id;
  uint8_t *vbuf;
  uint8_t data[4];
  
  id = motor_id + spd_mode_offset;
  vbuf=(uint8_t*)&vel;
  
  data[0] = *vbuf;
  data[1] = *(vbuf+1);
  data[2] = *(vbuf+2);
  data[3] = *(vbuf+3);
  
  BSP_CAN_SendData(hcan, id, data, 4);
  return 1;
}

uint8_t DM_DrivePOSCtrl(CAN_HandleTypeDef* hcan, uint16_t motor_id, float pos, float vel)
{
  uint16_t id = motor_id + pos_mode_offset;;

  uint8_t* pbuf=(uint8_t*)&pos;
  uint8_t* vbuf=(uint8_t*)&vel;
  
  uint8_t data[8];
  data[0] = *pbuf;
  data[1] = *(pbuf+1);
  data[2] = *(pbuf+2);
  data[3] = *(pbuf+3);
  data[4] = *vbuf;
  data[5] = *(vbuf+1);
  data[6] = *(vbuf+2);
  data[7] = *(vbuf+3);
  
  return canx_send_data(hcan, id, data, 8);
}

uint8_t DM_DriveMITCtrl (CAN_HandleTypeDef* hcan, uint16_t motor_id, 
                      float pos, float vel,float kp, float kd, float tor)
{
  uint8_t data[8];
  uint16_t pos_tmp,vel_tmp,kp_tmp,kd_tmp,tor_tmp;
  uint16_t id = motor_id + mit_mode_offset;

  pos_tmp = float_to_uint(pos, DM_POS_FLOAT_MIN, DM_POS_FLOAT_MAX, 16);
  vel_tmp = float_to_uint(vel, DM_VEL_FLOAT_MIN, DM_VEL_FLOAT_MAX, 12);
  tor_tmp = float_to_uint(tor, DM_TOR_FLOAT_MIN, DM_TOR_FLOAT_MAX, 12);
  kp_tmp  = float_to_uint(kp,  DM_KP_MIN, DM_KP_MAX, 12);
  kd_tmp  = float_to_uint(kd, DM_KD_MIN, DM_KD_MAX, 12);

  data[0] = (pos_tmp >> 8);
  data[1] = pos_tmp;
  data[2] = (vel_tmp >> 4);
  data[3] = ((vel_tmp&0xF)<<4)|(kp_tmp>>8);
  data[4] = kp_tmp;
  data[5] = (kd_tmp >> 4);
  data[6] = ((kd_tmp&0xF)<<4)|(tor_tmp>>8);
  data[7] = tor_tmp;
  
  return BSP_CAN_SendData(hcan, id, data, 8);
}

/**
* @brief:      	DM_WriteReg:  DM4310电机写寄存器
* @param[in]:   raw_id:       电机CAN ID
* @param[in]:   rid:          寄存器地址
* @param[in]:   d:            写入的数据(长度为4的数组)
* @retval:     	uint8_t       状态 (0:失败, 1:成功)
* @details:    	向寄存器写入数据
**/
uint8_t DM_WriteReg(uint16_t raw_id, uint8_t rid, uint8_t* d)
{
  if (raw_id > 0x7FF) {
    return 0; 
  }
  uint8_t can_id_l = raw_id & 0xFF;
  uint8_t can_id_h = (raw_id >> 8) & 0xFF;
  // 拼接数据帧，0x55为写入寄存器指令
  uint8_t data[8] = {can_id_l, can_id_h, DM_INS_WRITE, rid, d[0], d[1], d[2], d[3]};
  BSP_CAN_SendData(&hcan1, DM_STDID_BROAD, data, 8);
  
  return 1;
}

/**
* @brief:      	DM_SaveReg:   DM4310电机存寄存器
* @param[in]:   id:           电机CAN ID
* @param[in]:   rid:          寄存器地址
* @retval:     	uint8_t       状态 (0:失败, 1:成功)
* @details:    	保存写入的电机参数
**/
uint8_t DM_SaveReg(uint16_t id, uint8_t rid) 
{
	uint8_t can_id_l = id & 0xFF;       // 低 8 位
  uint8_t can_id_h = (id >> 8) & 0xFF; // 高 3 位
	
	uint8_t data[4] = {can_id_l, can_id_h, DM_INS_SAVE, rid};
	BSP_CAN_SendData(&hcan1, 0x7FF, data, 4);
  return 1;
}

int float_to_uint(float x_float, float x_min, float x_max, int bits)
{
  /* Converts a float to an unsigned int, given range and number of bits */
  float span = x_max - x_min;
  float offset = x_min;
  return (int) ((x_float-offset)*((float)((1<<bits)-1))/span);
}

float uint_to_float(int x_int, float x_min, float x_max, int bits)
{
  /* converts unsigned int to float, given range and number of bits */
  float span = x_max - x_min;
  float offset = x_min;
  return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
}

/**
 * @brief:    在位置模式下使其回到零点
 * @details:  需要更改模式
 */
uint8_t DM_GotoZero(motor_dm_t* dm_ptr)
{
  // 失能状态修改
  DM_Disable(dm_ptr);
  HAL_Delay(30);

  // 设置为 pos_mode
  uint8_t data[4] = {pos_mode, 0, 0, 0};
  DM_WriteReg(dm_ptr->id, DM_RID_CTRL_MODE, data);
  HAL_Delay(30);

  // 使能，转到 0 位置
  DM_Enable(dm_ptr);
  HAL_Delay(30);
  DM_DrivePOSCtrl(&hcan1, dm_ptr->id, 0, 3.14f);
  HAL_Delay(1500);

  // 回到 mit_mode
  DM_Disable(dm_ptr);
  HAL_Delay(30);
  data[0] = mit_mode;
  DM_WriteReg(dm_ptr->id, DM_RID_CTRL_MODE, data);
  HAL_Delay(30);

  return 1;
}

/**
 * @brief:    请求一次反馈帧
 */
uint8_t DM_RequestFB(motor_dm_t* dm_ptr)
{
  uint8_t data[8];
	data[0] = (uint8_t)(dm_ptr->id & 0xff);
	data[1] = (uint8_t)((dm_ptr->id >> 8) & 0xff);
	data[2] = 0xCC;
	data[3] = 0x00;
	data[4] = 0xFF;
	data[5] = 0xFF;
	data[6] = 0xFF;
	data[7] = 0xFF;
	canx_send_data(&hcan1, 0x7FF, data, 8);
	return 1;
}
